﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using GestureSaver;
using Microsoft.Kinect;

namespace Experimenter
{
    public static class NewDTW
    {
        public static JointType[] SelectedJoints = { JointType.HandLeft, JointType.ElbowLeft, JointType.HandRight, JointType.ElbowRight };

        public static double Distance(MSkeleton s1, MSkeleton s2, Dictionary<JointType, double[]> weights)
        {
            double d = 0;

            foreach (JointType j in SelectedJoints)
            {
                d += (s1[j].Position.X - s2[j].Position.X) * weights[j][0] + (s1[j].Position.Y - s2[j].Position.Y) * weights[j][1];
            }

            return d;
        }

        // Compute the min DTW distance between seq2 and all possible endings of seq1.
        public static double DTW(Gesture g1, Gesture g2, Dictionary<JointType, double[]> weights = null)
        {
            double[,] costs = new double[g1.SkeletonCount, g2.SkeletonCount];

            for (int i = 0; i < g1.SkeletonCount; i++)
                for (int j = 0; j < g2.SkeletonCount; j++)
                    costs[i, j] = double.PositiveInfinity;

            costs[0, 0] = Distance(g1[0], g2[0], weights);

            for (int i = 1; i < g1.SkeletonCount; i++) // first column
            {                
                costs[i, 0] = costs[i - 1, 0] + Distance(g1[i], g2[0], weights);        
            }

            for (int i = 1; i < g2.SkeletonCount; i++) // first row
            {                
                costs[0, i] = costs[0, i - 1] + Distance(g1[0], g2[i], weights);          
            }

            // horizontal, vertical
            double[] tempCosts = new double[2];

            for (int i = 1; i < g1.SkeletonCount; i++)
            {
                for (int j = 1; j < g2.SkeletonCount; j++)
                {
                    double newCost = Distance(g1[i], g2[j], weights);

                    for (int p = 0; p < 2; p++)
                        tempCosts[p] = Double.PositiveInfinity;

                    double oldCost = costs[i - 1, j - 1];

                    int minIndex = -1;

                    for (int p = 0; p < 2; p++)
                        if (tempCosts[p] < oldCost)
                            minIndex = p;

                    switch (minIndex)
                    {
                        case -1: // diagonal
                            oldCost = costs[i - 1, j - 1];
                            break;
                        case 0: // horizontal
                            oldCost = costs[i - 1, j];
                            break;
                        case 1: // vertical 
                            oldCost = costs[i, j - 1];
                            break;
                        default:
                            throw new Exception("Something is wrong");
                    }

                    costs[i, j] = oldCost + newCost;
                }
            }

            // Find best between seq2 and an ending (postfix) of seq1.
            double bestMatch = double.PositiveInfinity;

            for (int i = (g1.SkeletonCount * 3) / 4; i < g1.SkeletonCount; i++)
                if (Math.Abs(costs[i, g2.SkeletonCount - 1]) < bestMatch)
                    bestMatch = Math.Abs(costs[i, g2.SkeletonCount - 1]);

            for (int i = (g2.SkeletonCount * 3) / 4; i < g2.SkeletonCount; i++)
                if (Math.Abs(costs[g1.SkeletonCount - 1, i]) < bestMatch)
                    bestMatch = Math.Abs(costs[g1.SkeletonCount - 1, i]);

            return bestMatch;
        }
    }
}
